#include <buola/buola.h>

#if defined BUOLA_HAVE_OPENNI2

#include "ckinectdevice_openni2.h"
#include "ckinectdriver_openni2.h"

#include <buola/image/format.h>
#include <buola/image/io.h>

namespace buola { namespace cvi {

CKinectDevice::CKinectDevice(const openni::DeviceInfo &pInfo)
    :   mDeviceInfo(pInfo)
    ,   mInitialized(false)
    ,   mSyncIndex(0)
    ,   mStartCount(0)
    ,   mStarted(false)
{
    msg_info() << pInfo.getName() << "\n";
    msg_info() << pInfo.getUri() << "\n";
    msg_info() << pInfo.getUsbProductId() << "\n";
    msg_info() << pInfo.getUsbVendorId() << "\n";
    msg_info() << pInfo.getVendor() << "\n";
}

CKinectDevice::~CKinectDevice()
{
    
}

void CKinectDevice::Start()
{
    if(!mStartCount++)
    {
        lock_guard<mutex> lLock(mMutex);
        if(mThread.get_id()==thread::id())
            mThread=thread([this]{OnThread();});
        mCondVar.notify_one();
    }
}

void CKinectDevice::Stop()
{
    mStartCount--;
}

std::pair<img::CImage_rgb8,img::CImage_gray32f> CKinectDevice::GetOne()
{
    std::pair<img::CImage_rgb8,img::CImage_gray32f> lResult;
    Start();
    {
        unique_lock<mutex> lLock(mSyncMutex);
        int lSyncIndex=mSyncIndex;
        while(lSyncIndex==mSyncIndex)
            mSyncCondVar.wait(lLock);
        lResult.first=share(mSyncImageRGB);
        lResult.second=share(mSyncImageDepth);
    }
    Stop();
    
    return lResult;
}


bool CKinectDevice::HasDepthStream()
{
    if(!mInitialized)
        Initialize();

    return mNIDepth.isValid();
}

bool CKinectDevice::HasImageStream()
{
    if(!mInitialized) 
        Initialize();

    return mNIImage.isValid();
}

bool CKinectDevice::HasIRStream()
{
    if(!mInitialized) 
        Initialize();

    return mNIIR.isValid();
}

void CKinectDevice::Initialize()
{
    lock_guard<mutex> lLock(mMutex);
    if(mInitialized) return;

    openni::Status lStatus=mNIDevice.open(mDeviceInfo.getUri());
    if(lStatus!=openni::STATUS_OK)
        throw std::runtime_error("can't open NI device");

    lStatus=mNIDepth.create(mNIDevice,openni::SENSOR_DEPTH);
    if(lStatus!=openni::STATUS_OK)
        throw std::runtime_error("can't create depth stream");

    lStatus=mNIImage.create(mNIDevice,openni::SENSOR_COLOR);
    if(lStatus!=openni::STATUS_OK)
        throw std::runtime_error("can't create image stream");

    openni::VideoMode lVideoMode=mNIDepth.getVideoMode();
    msg_info() << "depth:" << lVideoMode.getResolutionX() << "x" << lVideoMode.getResolutionY() << "\n";
    lVideoMode=mNIImage.getVideoMode();
    msg_info() << "image:" << lVideoMode.getResolutionX() << "x" << lVideoMode.getResolutionY() << "\n";

    mNIDevice.setImageRegistrationMode(openni::IMAGE_REGISTRATION_DEPTH_TO_COLOR);
    
    mInitialized=true;
}

void CKinectDevice::OnThread()
{
    msg_info() << "onthread\n";
    try
    {
        int lFrameCount=0;
        
        while(true)
        {
            if(!mStartCount)
            {
                lFrameCount=0;
                
                unique_lock<mutex> lLock(mMutex);
                
                while(!mStartCount)
                {
                    if(mStarted)
                        DoStop();
                    
                    mCondVar.wait(lLock);
                }
            }
            
            if(!mStarted)
            {
                DoStart();
            }
            
            DoIteration(++lFrameCount);
        }
    }
    catch(std::exception &pE)
    {
        msg_info() << "got exception in thread " << pE.what() << "\n";
    }
}

void CKinectDevice::DoStart()
{
    if(!HasDepthStream())
        throw XInvalid("device doesn't have a depth stream");

    if(mNIDepth.start()!=openni::STATUS_OK)
        throw std::runtime_error("can't start depth stream");
    if(HasImageStream()&&mNIImage.start()!=openni::STATUS_OK)
        throw std::runtime_error("can't start depth stream");
    
    mStarted=true;
}

void CKinectDevice::DoStop()
{
    
}

void CKinectDevice::DoIteration(int pCount)
{
    openni::VideoStream *lStreams[]={&mNIDepth,&mNIImage};
    int lIndex;
    //wait for one
    openni::OpenNI::waitForAnyStream(lStreams,1,&lIndex);
    //wait for the other
    openni::OpenNI::waitForAnyStream(lStreams+1,1,&lIndex);
    
    openni::VideoFrameRef lDepthFrame,lImageFrame;
    mNIDepth.readFrame(&lDepthFrame);
    mNIImage.readFrame(&lImageFrame);

    const openni::DepthPixel *lDepthSrc=(const openni::DepthPixel*)lDepthFrame.getData();
    const uint8_t *lRGBSrc=(const uint8_t*)lImageFrame.getData();
    
    CSize_i lDepthSize(lDepthFrame.getWidth(),lDepthFrame.getHeight());
    CSize_i lRGBSize(lImageFrame.getWidth(),lImageFrame.getHeight());

    if(lDepthSize!=lRGBSize)
        throw XInvalid("CKinectDevice: image and depth sizes don't match");

    img::CImage_gray32f lDepth(lDepthSize);
    img::CImage_rgb8 lRGB(lRGBSize);
    
    auto lDepthDst=lDepth.RawData();
    auto lRGBDst=lRGB.RawData();

    for(int y=0;y<lRGBSize.y;y++)
    {
        for(int x=0;x<lRGBSize.x;x++)
        {
            auto lDepthValue=*lDepthSrc++;
            if(lDepthValue==0)
            {
                *lDepthDst++=NAN;
            }
            else
            {
                *lDepthDst++=lDepthValue/1000.0;
            }
            
            *lRGBDst++=*lRGBSrc++;
            *lRGBDst++=*lRGBSrc++;
            *lRGBDst++=*lRGBSrc++;
        }
    }
    lRGB.TimeStampNow();
    
    sImages(lRGB,lDepth);

    if(pCount>10)
    {
        lock_guard<mutex> lLock(mSyncMutex);
    
        mSyncIndex++;
        mSyncImageRGB=std::move(lRGB);
        mSyncImageDepth=std::move(lDepth);
        mSyncCondVar.notify_all();
    } 
    else
    {
        msg_info() << "ignoring one\n";
    }
}

/*namespace cvi*/ } /*namespace buola*/ }

#endif
